#include "SLAM/common.hpp"
#include "SLAM/landmark_manager.hpp"
#include "SLAM/matcher.hpp"
#include "SLAM/ransac_se2.hpp"
#include "SLAM/icp_se2.hpp"

// 继续使用已有的检测 & 匈牙利算法文件：
#include "reflector_detector.hpp"   // 原有头文件
#include "HungarianAlgorithm.hpp"   // 原有匈牙利算法头（仅当在检测内部用到时）
#include "params_detector.hpp"
using std::placeholders::_1;
// ==================== 点类型定义 ====================
using PointT = pcl::PointXYZI;
using CloudT = pcl::PointCloud<PointT>;


namespace SLAM {

// --- SE2 表示 & 工具 ---
struct SE2 {
  double x{0.0}, y{0.0}, yaw{0.0};  // map->base
};

class ReflectorDetectorNode : public rclcpp::Node {
public:
  explicit ReflectorDetectorNode(const rclcpp::NodeOptions& opts = {})
  : rclcpp::Node("reflector_detector_node", opts),
    lm_mgr_(std::make_unique<LandmarkManager>(LandmarkManager::Params{}, get_clock()))
  {
    // ------- 参数读取 -------
    declare_parameter<std::string>("input_cloud", params_.input_cloud_topic_);
    get_parameter("input_cloud", params_.input_cloud_topic_);

    declare_parameter<std::string>("map_frame", map_frame_);
    declare_parameter<std::string>("base_frame", base_frame_);
    declare_parameter<std::string>("odom_frame", odom_frame_);
    get_parameter("map_frame", map_frame_);
    get_parameter("base_frame", base_frame_);
    get_parameter("odom_frame", odom_frame_);

    //----------------- 雷达安装参数 -----------------
    declare_parameter<double>("laser_in_base_tx", t_lb_.x());
    declare_parameter<double>("laser_in_base_ty", t_lb_.y());
    declare_parameter<double>("laser_in_base_yaw", yaw_lb_);
    declare_parameter<std::string>("extrinsic_version", extrinsic_version_);
    get_parameter("laser_in_base_tx", t_lb_.x());
    get_parameter("laser_in_base_ty", t_lb_.y());
    get_parameter("laser_in_base_yaw", yaw_lb_);
    get_parameter("extrinsic_version", extrinsic_version_);

    // LandmarkManager 参数
    lm_params_ = LMParams{};
    declare_parameter<int>("min_hits_confirm", lm_params_.min_hits_confirm);
    declare_parameter<double>("max_age_sec", lm_params_.max_age_sec);
    declare_parameter<double>("merge_radius", lm_params_.merge_radius);
    declare_parameter<double>("nn_gate_m", lm_params_.nn_gate_m);
    declare_parameter<bool>("use_ekf", lm_params_.use_ekf);
    declare_parameter<double>("q_process", lm_params_.q_process);
    declare_parameter<double>("r_meas", lm_params_.r_meas);
    get_parameter("min_hits_confirm", lm_params_.min_hits_confirm);
    get_parameter("max_age_sec", lm_params_.max_age_sec);
    get_parameter("merge_radius", lm_params_.merge_radius);
    get_parameter("nn_gate_m", lm_params_.nn_gate_m);
    get_parameter("use_ekf", lm_params_.use_ekf);
    get_parameter("q_process", lm_params_.q_process);
    get_parameter("r_meas", lm_params_.r_meas);

    // Matcher 参数
    matcher_params_ = MatcherParams{};
    declare_parameter<double>("mahalanobis_gate2", matcher_params_.mahalanobis_gate2);
    declare_parameter<double>("obs_sigma_xy", matcher_params_.obs_sigma_xy);
    get_parameter("mahalanobis_gate2", matcher_params_.mahalanobis_gate2);
    get_parameter("obs_sigma_xy", matcher_params_.obs_sigma_xy);

    // 位姿质量门
    declare_parameter<double>("pose_rmse_gate", pose_rmse_gate_);    // m
    declare_parameter<int>("pose_min_inliers", pose_min_inliers_);
    get_parameter("pose_rmse_gate", pose_rmse_gate_);
    get_parameter("pose_min_inliers", pose_min_inliers_);

    // ---- 反光柱识别参数 ----
    declare_parameter<std::string>("params_input_cloud", params_.input_cloud_topic_);
    declare_parameter<std::string>("params_marker_ns", params_.marker_ns_);
    declare_parameter<std::string>("params_target_frame", params_.target_frame_); // map
    declare_parameter<std::string>("params_laser_frame",  params_.laser_frame_);  // laser

    declare_parameter<double>("params_z_min", params_.z_min_);
    declare_parameter<double>("params_z_max", params_.z_max_);
    declare_parameter<double>("params_range_min", params_.range_min_);
    declare_parameter<double>("params_range_max", params_.range_max_);

    declare_parameter<double>("params_coarse_r0", params_.coarse_r0_);
    declare_parameter<double>("params_coarse_i0", params_.coarse_i0_);
    declare_parameter<double>("params_coarse_r1", params_.coarse_r1_);
    declare_parameter<double>("params_coarse_i1", params_.coarse_i1_);

    declare_parameter<double>("params_bin_size", params_.bin_size_);
    declare_parameter<int>("params_min_points_per_bin", params_.min_points_per_bin_);
    declare_parameter<double>("params_k_sigma", params_.k_sigma_);
    declare_parameter<double>("params_k_alpha", params_.k_alpha_);



    declare_parameter<double>("params_cluster_tolerance", params_.cluster_tolerance_);
    declare_parameter<int>("params_min_cluster_size", params_.min_cluster_size_);
    declare_parameter<int>("params_max_cluster_size", params_.max_cluster_size_);

    declare_parameter<double>("params_nominal_radius", params_.nominal_radius_);
    declare_parameter<double>("params_radius_gate_ratio", params_.radius_gate_ratio_);
    declare_parameter<double>("params_circle_inlier_eps", params_.circle_inlier_eps_);

    declare_parameter<bool>("params_enable_near_recheck", params_.enable_near_recheck_);
    declare_parameter<double>("params_near_range", params_.near_range_);
    declare_parameter<double>("params_near_low_ratio", params_.near_low_ratio_);
    declare_parameter<double>("params_near_i_trapezoid", params_.near_i_trapezoid_);
    

    declare_parameter<double>("params_landmark_translation_weight", params_.lm_translation_weight_);
    declare_parameter<double>("params_landmark_rotation_weight", params_.lm_rotation_weight_);

    declare_parameter<double>("params_max_age", params_.max_age_);
    declare_parameter<double>("params_decay_threshold", params_.decay_threshold_);
    declare_parameter<double>("params_decay_factor", params_.decay_factor_);
    declare_parameter<int>("params_min_hits", params_.min_hits_);
    declare_parameter<double>("params_max_distance_threshold", params_.max_distance_threshold_);



    // get params
    get_parameter("params_input_cloud", params_.input_cloud_topic_);
    get_parameter("params_marker_ns", params_.marker_ns_);
    get_parameter("params_target_frame", params_.target_frame_);
    get_parameter("params_laser_frame",  params_.laser_frame_);

    get_parameter("params_z_min", params_.z_min_);
    get_parameter("params_z_max", params_.z_max_);
    get_parameter("params_range_min", params_.range_min_);
    get_parameter("params_range_max", params_.range_max_);

    get_parameter("params_coarse_r0", params_.coarse_r0_);
    get_parameter("params_coarse_i0", params_.coarse_i0_);
    get_parameter("params_coarse_r1", params_.coarse_r1_);
    get_parameter("params_coarse_i1", params_.coarse_i1_);

    get_parameter("params_bin_size", params_.bin_size_);
    get_parameter("params_min_points_per_bin", params_.min_points_per_bin_);
    get_parameter("params_k_sigma", params_.k_sigma_);
    get_parameter("params_k_alpha", params_.k_alpha_);
    


    get_parameter("params_cluster_tolerance", params_.cluster_tolerance_);
    get_parameter("params_min_cluster_size", params_.min_cluster_size_);
    get_parameter("params_max_cluster_size", params_.max_cluster_size_);

    get_parameter("params_nominal_radius", params_.nominal_radius_);
    get_parameter("params_radius_gate_ratio", params_.radius_gate_ratio_);
    get_parameter("params_circle_inlier_eps", params_.circle_inlier_eps_);

    get_parameter("params_enable_near_recheck", params_.enable_near_recheck_);
    get_parameter("params_near_range", params_.near_range_);
    get_parameter("params_near_low_ratio", params_.near_low_ratio_);
    get_parameter("params_near_i_trapezoid", params_.near_i_trapezoid_);

    

    get_parameter("params_landmark_translation_weight", params_.lm_translation_weight_);
    get_parameter("params_landmark_rotation_weight", params_.lm_rotation_weight_);

    get_parameter("params_max_age", params_.max_age_);
    get_parameter("params_decay_threshold", params_.decay_threshold_);
    get_parameter("params_decay_factor", params_.decay_factor_);
    get_parameter("params_min_hits", params_.min_hits_);
    get_parameter("params_max_distance_threshold", params_.max_distance_threshold_);


    // 应用参数
    lm_mgr_ = std::make_unique<LandmarkManager>(lm_mgr_params_, get_clock());

    // ---------------- TF ----------------
    tf_buffer_      = std::make_shared<tf2_ros::Buffer>(get_clock());
    tf_listener_    = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
    tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);

    ///cx/cloud 的 frame_id=laser_link 会通过 TF 链正确叠加到 map
    // 静态 TF：base_link -> laser_link
    static_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*this);

    //发布静态TF 树   雷达系->机器人系
    geometry_msgs::msg::TransformStamped tf_bl_laser;
    tf_bl_laser.header.stamp = now();
    tf_bl_laser.header.frame_id = base_frame_;   // parent
    tf_bl_laser.child_frame_id  = params_.laser_frame_.empty() ? "laser_link" : params_.laser_frame_;
    tf_bl_laser.transform.translation.x = t_lb_.x();
    tf_bl_laser.transform.translation.y = t_lb_.y();
    tf_bl_laser.transform.translation.z = 0.0;
    tf2::Quaternion q_bl_laser; q_bl_laser.setRPY(0,0,yaw_lb_);
    tf_bl_laser.transform.rotation = tf2::toMsg(q_bl_laser);
    static_broadcaster_->sendTransform(tf_bl_laser);



    // ---------------- 订阅/发布 ----------------
    sub_cloud_ = create_subscription<sensor_msgs::msg::PointCloud2>(
      params_.input_cloud_topic_, rclcpp::SensorDataQoS(),
      std::bind(&ReflectorDetectorNode::onCloud, this, std::placeholders::_1));
/*
    sub_odom_ = create_subscription<nav_msgs::msg::Odometry>(
      odom_topic_, 50, std::bind(&ReflectorDetectorNode::onOdom, this, std::placeholders::_1));

*/
    pub_markers_   = create_publisher<visualization_msgs::msg::MarkerArray>("/reflector_markers", 10);
    pub_landmarks_ = create_publisher<visualization_msgs::msg::MarkerArray>("/global_landmarks", 10);
    pub_pose_      = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("/current_pose", 10);
    pub_odom_ = create_publisher<nav_msgs::msg::Odometry>("/odom", 10);

    RCLCPP_INFO(get_logger(), "SLAM 节点启动：反光柱识别 + 地标维护 + 位姿解算. Extrinsic version: %s",
                extrinsic_version_.c_str());
  }

private:
  // ---------------- 里程计回调 ----------------
  void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg) {
    last_odom_ = *msg;
    have_odom_ = true;
  }

  // ---------------- 点云回调（核心流程） ----------------
  void onCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg) {
    // 1) 点云转 PCL
    CloudT::Ptr cloud(new CloudT);
    pcl::fromROSMsg(*msg, *cloud);
////////////////////////////////////////////////////////
  // 更新并打印“到目前为止”的全局 rmin/rmax 及其强度
  double rmin_,rmax_,rmin_intensity_,rmax_intensity_,mu;
  for (const auto& p : *cloud) {
    const double r = std::hypot(p.x, p.y);
    mu+=p.intensity;
    if (r < rmin_) { rmin_ = r; rmin_intensity_ = p.intensity; }
    if (r > rmax_) { rmax_ = r; rmax_intensity_ = p.intensity; }
  }
  mu /=static_cast<int>(cloud->points.size());
  if (!std::isfinite(rmin_) || !std::isfinite(rmax_)) return;
  /*
  RCLCPP_INFO_THROTTLE(
      this->get_logger(), *this->get_clock(), 1000,
      "累计强度-距离统计：r_min=%.3f (I=%.1f), r_max=%.3f (I=%.1f),mu = %.1f",
      rmin_, rmin_intensity_, rmax_, rmax_intensity_ ,mu);
*/
////////////////////////////////////////////////////////////
    // 1) 反光柱检测
    // - passThroughZ / coarseFilter / fineFilterInBins / clusterAndFit
    reflector_slam_system::ReflectorDetector detector(get_node_options() , params_); // 保持原接口
    detector.passThroughZ(cloud);

    CloudT::Ptr coarse(new CloudT);
    std::vector<size_t> kept_idx;
    detector.coarseFilter(cloud, coarse, kept_idx);
    if (coarse->empty()) { publishViz({}, {}, {}, {}, msg->header); return; }

    std::vector<int> mask_keep(coarse->size(), 0);
    detector.fineFilterInBins(coarse, kept_idx, mask_keep);
    
    // 2) 若衰减模型已准备就绪，基于（距离-强度）模型进行“第二次阈值收紧”(待定功能)
    if (lm_mgr_->attenuation().ready()) {
      for (size_t i=0;i<coarse->size(); ++i) {
        if (!mask_keep[i]) continue;
        const auto &p = (*coarse)[i];
        const double r = std::hypot(p.x, p.y);
        const double thr = lm_mgr_->attenuation().threshold(r, 2.5);
        if (double(p.intensity) < thr) mask_keep[i] = 0;
      }
    }

    // 3) 聚类 + 圆拟合（得到base系下的圆心/半径/强度）
    std::vector<Eigen::Vector2d> centers_eig;
    std::vector<double> radii, strengths;
    detector.clusterAndFit(coarse, mask_keep, centers_eig, radii, strengths);
/*
    for(auto center : centers_eig){
      printf("识别到的当前帧反光柱   坐标：  x = %.3f, y=%.3f\n",
            center.x(), center.y());
    }
    for(auto r : radii) printf("半径 ：  r = %.3f\n", r);
    for(auto i : strengths) printf("强度 ：  strengths = %.3f\n",i);
*/
    printf("center size ：  centers_eig.size = %d\n", centers_eig.size());
    printf("半径 size ：  r.size = %d\n", radii.size());
    printf("indensity size ：  i.size = %d\n", strengths.size());
    //==========新增雷达系观测 → 机器人系（先用固定外参 t_lb_, yaw_lb_ 做一次变换）================
    std::vector<Reflector> obs_base; obs_base.reserve(centers_eig.size());
    for (size_t i=0; i<centers_eig.size(); ++i) {
      Eigen::Vector2d p_l(centers_eig[i].x(), centers_eig[i].y()); // 雷达系
      const double r = (i < radii.size()? radii[i] : 0.0);
      const double I = (i < strengths.size()? strengths[i] : 0.0);
      obs_base.push_back(laserToBase(p_l, t_lb_, yaw_lb_, r, I));   //  机器人系

      printf("识别到的当前帧反光柱   坐标：  x = %.3f, y=%.3f\n",
            centers_eig[i].x(), centers_eig[i].y());
      printf("半径 ：  r = %.3f\n", r);
      printf("强度 ：  strengths = %.3f\n",I);

    }

    // 4) 预测：A1′ = A_prev_ ⊕ delta_prev_
    //SE2 A1_prior = se2Compose(A_prev_, delta_prev_);
    SE2 A1_prior = last_pose_;

    // 5) 观测从 base 系转换到 map 系（用先验 A1′）
    std::vector<Reflector> obs_map = transformBaseObsToMap(obs_base, A1_prior);
    // 若无法转换（TF 不可用），直接退出（可视化当前帧）
    if (obs_map.empty()) { publishViz(obs_base, {}, lm_mgr_->all(), {}, msg->header); return; }


    // 6)  ==================  预门 + 最近邻 + 匈牙利 + 二次门 ==================
    const auto lms_confirm = lm_mgr_->confirmed();
    MahalanobisMatcher matcher(matcher_params_);
    std::vector<Cov2> covs = lm_mgr_->all_cov_by_order(lms_confirm);
    const auto matches = matcher.match(lms_confirm, covs, obs_map);

    // 7) RANSAC 位姿估计 -> ICP 细化
    geometry_msgs::msg::Pose mTb; bool have_pose=false; double rmse=1e9;
    if (matches.size() >= 3){
      auto sol = ransacSE2(obs_base, lms_confirm, matches, 0.20, 120);
      if (!sol.inliers.empty()){
        // ICP 细化
        icp_refine_se2(obs_base, lms_confirm, matches, sol.inliers, sol.x, sol.y, sol.yaw, 6);
        rmse = sol.rmse;
        mTb.position.x = sol.x; mTb.position.y = sol.y; mTb.position.z = 0.0;
        tf2::Quaternion q; q.setRPY(0,0,sol.yaw);
        mTb.orientation = tf2::toMsg(q);
        have_pose = true;
      }
    }

    // 8) 质量门（匹配数 & RMSE）
    const bool high_conf = (have_pose && (int)matches.size() >= pose_min_inliers_ && rmse <= pose_rmse_gate_);

    // 9) 用高可信位姿更新“地标 Landmark”（否则只维护命中/时间，不改位置）
    lm_mgr_->set_now(this->now().seconds());
    std::vector<bool> is_matched(obs_map.size(), false);
    for (auto &mp:matches) {
      if (mp.obs_index>=0 && mp.obs_index<(int)is_matched.size()) 
        is_matched[mp.obs_index]=true;
    }
    lm_mgr_->fuse(obs_map, matches, high_conf);
    lm_mgr_->spawn_unmatched(obs_map, is_matched, high_conf);


    //======注意：如果上面的匹配方法失败（sel<3 或 rmse过大），回退使用ICP 匹配========
    // if (!have_pose) { have_pose = trySolveByRansacInBase(...); }
    if (have_pose) {
      // 把 mTb -> SE2 A1（map→base 的当前帧绝对位姿）
      //SE2 A1;
      last_pose_.x = mTb.position.x;
      last_pose_.y = mTb.position.y;
      tf2::Quaternion q;
      tf2::fromMsg(mTb.orientation, q);
      double roll, pitch, yaw;
      tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
      last_pose_.yaw = yaw;
      
      // 发布 map->base
      geometry_msgs::msg::TransformStamped tf_mb;
      tf_mb.header.stamp = msg->header.stamp;
      tf_mb.header.frame_id = map_frame_;
      tf_mb.child_frame_id  = base_frame_;
      tf_mb.transform.translation.x = last_pose_.x;
      tf_mb.transform.translation.y = last_pose_.y;
      tf_mb.transform.translation.z = 0.0;
      tf2::Quaternion q; q.setRPY(0,0,last_pose_.yaw);
      tf_mb.transform.rotation = tf2::toMsg(q);
      tf_broadcaster_->sendTransform(tf_mb);

      // 发布 current_pose
      geometry_msgs::msg::PoseWithCovarianceStamped pwc;
      pwc.header = msg->header; pwc.header.frame_id = map_frame_;
      pwc.pose.pose = mTb;
      pub_pose_->publish(pwc);

/*
      if (!initialized_) {
        // ----- 首次对齐：让 odom 与 map 完全重合 -----
        A_prev_      = A1;          // 上一帧后验直接设为当前解
        odom_accum_  = A1;          // 里程计原点/朝向对齐到 map
        delta_prev_  = {0, 0, 0};   // 首帧不累计相对增量
        //publishPoseAndTF(mTb, msg->header.stamp);  // 此时 map->odom 恒为单位
        initialized_ = true;
        return; // 首帧到此结束（避免又走一次累计）
      }

      // ----- 之后帧：正常用相对增量累计 -----
      SE2 theta1 = se2Between(A_prev_, A1);   // base_{k-1}→base_k 的相对运动（表达在上一帧局部）
      delta_prev_ = theta1;                   // 供下一帧的先验预测
      odom_accum_ = se2Compose(odom_accum_, theta1);
      A_prev_     = A1;

      //publishPoseAndTF(mTb, msg->header.stamp);
*/
    }
    //=====================================================================
    // 9) 可视化（当前帧/全局地标/关联线，颜色区分）  注意只显示confirm 中的地标
    publishViz(obs_base, obs_map, lm_mgr_->confirmed(), matches, msg->header);

  }

  // ---------------- 基座观测 -> 地图系转换（用先验 A1′） ----------------
  std::vector<Reflector> transformBaseObsToMap(const std::vector<Reflector>& obs_base,
                                              const SE2& prior)
  {
    std::vector<Reflector> obs_map; obs_map.reserve(obs_base.size());
    const double c=std::cos(prior.yaw), s=std::sin(prior.yaw);
    for (auto &o : obs_base){
      Reflector m;
      m.x = c*o.x - s*o.y + prior.x;
      m.y = s*o.x + c*o.y + prior.y;
      m.radius=o.radius; m.strength=o.strength;
      obs_map.push_back(m);
    }
    return obs_map;
  }


/*
  std::vector<Reflector> transformBaseObsToMap(const std::vector<Reflector>& obs_base) {
    std::vector<Reflector> out; out.reserve(obs_base.size());
    geometry_msgs::msg::TransformStamped T;
    bool ok = false;
    //这儿尤其要注意，考虑要不要改成上一帧计算出来的位姿   还是继续用最新的可用变换
    try { T = tf_buffer_->lookupTransform(map_frame_, base_frame_, tf2::TimePointZero); ok = true; }
    catch (...) { ok = false; }
    if (!ok) return out;

    tf2::Quaternion q;
    tf2::fromMsg(T.transform.rotation, q);
    double roll, pitch, yaw;
    tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
    const double c = std::cos(yaw), s = std::sin(yaw);
    const double tx = T.transform.translation.x;
    const double ty = T.transform.translation.y;

    for (auto &o : obs_base) {
      Reflector m;
      m.x = c*o.x - s*o.y + tx;
      m.y = s*o.x + c*o.y + ty;
      m.radius = o.radius;
      m.strength = o.strength;
      out.push_back(m);
    }
    return out;
  }
*/
  // ---------------- 发布位姿/TF ----------------
// ---------------- 发布位姿/TF ----------------
void publishPoseAndTF(const geometry_msgs::msg::Pose& mTb, const rclcpp::Time& stamp) {
  // 1) 构造 "map→base_link"（只用于内部计算，不广播！）
  tf2::Transform T_map_base;
  {
    geometry_msgs::msg::Transform tf_mb;
    tf_mb.translation.x = mTb.position.x;
    tf_mb.translation.y = mTb.position.y;
    tf_mb.translation.z = 0.0;
    tf_mb.rotation      = mTb.orientation;
    tf2::fromMsg(tf_mb, T_map_base);
  }

  // 2) 广播 "odom→base_link"（来自你的里程计 odom_accum_）
  geometry_msgs::msg::TransformStamped tf_ob;
  tf_ob.header.stamp = stamp;
  tf_ob.header.frame_id = odom_frame_;      // "odom"
  tf_ob.child_frame_id  = base_frame_;      // "base_link"
  tf2::Quaternion q_ob; q_ob.setRPY(0,0,odom_accum_.yaw);
  tf_ob.transform.translation.x = odom_accum_.x;
  tf_ob.transform.translation.y = odom_accum_.y;
  tf_ob.transform.translation.z = 0.0;
  tf_ob.transform.rotation      = tf2::toMsg(q_ob);
  tf_broadcaster_->sendTransform(tf_ob);

  // 3) 由 mTb 与 odom_accum_ 计算 "map→odom"，并广播
  tf2::Transform T_odom_base;
  tf2::fromMsg(tf_ob.transform, T_odom_base);   // 刚发出去的 odom→base_link
  tf2::Transform T_map_odom = T_map_base * T_odom_base.inverse();

  geometry_msgs::msg::TransformStamped tf_mo;
  tf_mo.header.stamp = stamp;
  tf_mo.header.frame_id = map_frame_;          // "map"
  tf_mo.child_frame_id  = odom_frame_;         // "odom"
  tf_mo.transform = tf2::toMsg(T_map_odom);
  tf_broadcaster_->sendTransform(tf_mo);

  // （可选）同步发 nav_msgs/Odometry：odom 为父，base_link 为子
  if (pub_odom_) {
    nav_msgs::msg::Odometry odom_msg;
    odom_msg.header.stamp = stamp;
    odom_msg.header.frame_id = odom_frame_;
    odom_msg.child_frame_id  = base_frame_;
    odom_msg.pose.pose.position.x = odom_accum_.x;
    odom_msg.pose.pose.position.y = odom_accum_.y;
    tf2::Quaternion q; q.setRPY(0,0,odom_accum_.yaw);
    odom_msg.pose.pose.orientation = tf2::toMsg(q);
    pub_odom_->publish(odom_msg);
  }

  debugPrintPoses(stamp, mTb);
}



  // ---------------- 可视化 ----------------
  void publishViz(const std::vector<Reflector>& obs_base,
                  const std::vector<Reflector>& obs_map,
                  const std::vector<Landmark>&  lms,
                  const std::vector<MatchPair>& matches,
                  const std_msgs::msg::Header&  hdr) {
    visualization_msgs::msg::MarkerArray ma;

    // 当前帧反光柱（基座系）— 青色（Cyan）
    {
      visualization_msgs::msg::Marker m;
      m.header = hdr; m.header.frame_id = base_frame_;
      m.ns = "current_reflectors"; m.id = 0;
      m.type = visualization_msgs::msg::Marker::SPHERE_LIST;
      m.action = visualization_msgs::msg::Marker::ADD;
      m.scale.x = 0.08; m.scale.y = 0.08; m.scale.z = 0.08;
      m.color.r = 0.0; m.color.g = 1.0; m.color.b = 1.0; m.color.a = 0.9;
      m.pose.orientation.w = 1.0;
      for (auto &o : obs_base) { geometry_msgs::msg::Point p; p.x=o.x; p.y=o.y; p.z=0; m.points.push_back(p); }
      ma.markers.push_back(m);     // 机器人系注释掉不显示
    }

    // 全局地标（地图系）— 橙色（Orange）
    {
      visualization_msgs::msg::Marker m;
      m.header = hdr; m.header.frame_id = map_frame_;
      m.ns = "global_landmarks"; m.id = 1;
      m.type = visualization_msgs::msg::Marker::SPHERE_LIST;
      m.action = visualization_msgs::msg::Marker::ADD;
      m.scale.x = 0.12; m.scale.y = 0.12; m.scale.z = 0.12;
      m.color.r = 1.0; m.color.g = 0.5; m.color.b = 0.0; m.color.a = 0.95;
      m.pose.orientation.w = 1.0;
      for (auto &L : lms) { geometry_msgs::msg::Point p; p.x=L.x; p.y=L.y; p.z=0; m.points.push_back(p); }
      ma.markers.push_back(m);  //confirm中的确认地标，显示，但是下面显示过，这里不显示
    }

    // 关联连线（地图系）— 绿色线段
    if (!matches.empty()) {
      std::unordered_map<int, Landmark> mp;
      for (auto &L : lms) mp[L.id] = L;

      visualization_msgs::msg::Marker m;
      m.header = hdr; m.header.frame_id = map_frame_;    //   map_frame_
      m.ns = "associations"; m.id = 2;
      m.type = visualization_msgs::msg::Marker::LINE_LIST;
      m.action = visualization_msgs::msg::Marker::ADD;
      m.scale.x = 0.02;
      m.color.r = 0.2; m.color.g = 1.0; m.color.b = 0.2; m.color.a = 0.8;
      m.pose.orientation.w = 1.0;

      for (auto &pair : matches) {
        auto it = mp.find(pair.lm_index); if (it == mp.end()) continue;
        if (pair.obs_index >= 0 && pair.obs_index < (int)obs_map.size()) {
          geometry_msgs::msg::Point a, b;
          a.x = it->second.x; a.y = it->second.y; a.z = 0;
          b.x = obs_map[pair.obs_index].x; b.y = obs_map[pair.obs_index].y; b.z = 0;
          m.points.push_back(a); m.points.push_back(b);
        }
      }
      ma.markers.push_back(m);
    }

    //pub_markers_->publish(ma);

    // 单独再发一份地标 MarkerArray（方便在 RViz 开关显示）
    visualization_msgs::msg::MarkerArray lm_arr;
    if (!lms.empty()) {
      visualization_msgs::msg::Marker m;
      m.header = hdr; m.header.frame_id = map_frame_;
      m.ns = "lm_points"; m.id = 100;
      m.type = visualization_msgs::msg::Marker::SPHERE_LIST;
      m.action = visualization_msgs::msg::Marker::ADD;

      const double dia = std::max(0.05, 2.0 * params_.nominal_radius_);
      m.scale.x = dia;  // 球直径
      m.scale.y = dia;
      m.scale.z = dia;      
      m.color.r = 1.0; 
      m.color.g = 0.5; 
      m.color.b = 0.0; 
      m.color.a = 0.95;
      m.pose.orientation.w = 1.0;
      m.lifetime = rclcpp::Duration(0, 0); // 不自动消失
      for (auto &L : lms) { geometry_msgs::msg::Point p; p.x=L.x; p.y=L.y; p.z=0; m.points.push_back(p); }
      lm_arr.markers.push_back(m);
    }
    pub_landmarks_->publish(lm_arr);
  }

  // 里程计转 TF Transform（用于 map->odom 计算）
  static geometry_msgs::msg::Transform odomToTF(const nav_msgs::msg::Odometry& od) {
    geometry_msgs::msg::Transform t;
    t.translation.x = od.pose.pose.position.x;
    t.translation.y = od.pose.pose.position.y;
    t.translation.z = od.pose.pose.position.z;
    t.rotation = od.pose.pose.orientation;
    return t;
  }

void debugPrintPoses(const rclcpp::Time& stamp,
                     const geometry_msgs::msg::Pose& mTb)
{
  // 1) 内部值
  double yaw_int = tf2::getYaw(mTb.orientation);
  printf("==== [POSE DEBUG] ==============================\n");
  printf("[INTERNAL] map->base:  x=%.3f  y=%.3f  yaw=%.3f rad\n",
         mTb.position.x, mTb.position.y, yaw_int);
  printf("[INTERNAL] odom->base: x=%.3f  y=%.3f  yaw=%.3f rad\n",
         odom_accum_.x, odom_accum_.y, odom_accum_.yaw);

  // 2) 用同一个 stamp 查 TF（等最多 0.1s 让 TF 进缓存）
  const rclcpp::Duration timeout = rclcpp::Duration::from_seconds(0.1);

  auto print_tf_xyyaw = [&](const std::string& parent, const std::string& child, const char* tag)
    -> std::optional<std::tuple<double,double,double>>
  {
    if (!tf_buffer_->canTransform(parent, child, stamp, timeout)) {
      printf("[TF   ] %s %s->%s lookup FAILED at t=%.3f\n",
             tag, parent.c_str(), child.c_str(), stamp.seconds());
      return std::nullopt;
    }
    auto T = tf_buffer_->lookupTransform(parent, child, stamp);
    double yaw = tf2::getYaw(T.transform.rotation);
    printf("[TF   ] %s %s->%s: x=%.3f  y=%.3f  yaw=%.3f rad\n",
           tag, parent.c_str(), child.c_str(),
           T.transform.translation.x, T.transform.translation.y, yaw);
    return std::make_tuple(T.transform.translation.x,
                           T.transform.translation.y,
                           yaw);
  };

  auto tf_mb = print_tf_xyyaw(map_frame_,  base_frame_,  "map->base");
  auto tf_ob = print_tf_xyyaw(odom_frame_, base_frame_,  "odom->base");
  auto tf_mo = print_tf_xyyaw(map_frame_,  odom_frame_,  "map->odom");

  // 3) 差异对比（仅在拿到 TF 的情况下）
  if (tf_mb) {
    auto [x_tf, y_tf, yaw_tf] = *tf_mb;
    double dyaw = std::remainder(yaw_tf - yaw_int, 2*M_PI);
    printf("[DIFF ] (map->base) TF-INT: dx=%.4f  dy=%.4f  dyaw=%.4f rad\n",
           x_tf - mTb.position.x, y_tf - mTb.position.y, dyaw);
  }
  if (tf_ob) {
    auto [x_tf, y_tf, yaw_tf] = *tf_ob;
    double dyaw = std::remainder(yaw_tf - odom_accum_.yaw, 2*M_PI);
    printf("[DIFF ] (odom->base) TF-INT: dx=%.4f  dy=%.4f  dyaw=%.4f rad\n",
           x_tf - odom_accum_.x, y_tf - odom_accum_.y, dyaw);
  }
  if (tf_mo) {
    auto [x_tf, y_tf, yaw_tf] = *tf_mo;
    printf("[CHK  ] map->odom ~ Identity?  |t|=%.6f  |yaw|=%.6f rad\n",
           std::hypot(x_tf, y_tf), std::fabs(std::remainder(yaw_tf, 2*M_PI)));
  }
  printf("===============================================\n");
}  

private:
  bool initialized_{false}; // add initialized
  paramsDetector params_;
  LMParams lm_params_;
  MatcherParams matcher_params_;

  std::string map_frame_{"map"};
  std::string base_frame_{"base_link"};
  std::string odom_frame_{"odom"};

  Eigen::Vector2d t_lb_{0.3625, 0.0};
  double yaw_lb_{3.1415926};
  std::string extrinsic_version_{"v1"};

  std::unique_ptr<LandmarkManager> lm_mgr_;

  // 质量门
  double pose_rmse_gate_{0.20};
  int    pose_min_inliers_{3};

  // 状态
  SE2 last_pose_{0,0,0};

  // ROS
  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_cloud_;
  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_markers_;
  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_landmarks_;
  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pub_pose_;

  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;  // tf tree
  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_broadcaster_;  // static tf tree

  


  //==================新增成员变量=================
  // ---- 内部里程计（程序自算）与先验/后验 ----
  SE2 A_prev_{0,0,0};     // 上一帧后验 A0
  SE2 delta_prev_{0,0,0}; // 上一帧计算出的“里程计增量” θ0
  SE2 odom_accum_{0,0,0}; // 里程计累计

  // ---- 雷达到基座外参（固定）----
  // tx=0.3625, ty=0, yaw=pi (roll/pitch 这里都0，2D）
  Eigen::Vector2d t_lb_{0.3625, 0.0};
  double yaw_lb_{3.14159};


  //===================新增工具函数===============

  static SE2 se2Compose(const SE2& a, const SE2& b) {
    // a ⊕ b
    const double c = std::cos(a.yaw), s = std::sin(a.yaw);
    SE2 out;
    out.x   = a.x + c*b.x - s*b.y;
    out.y   = a.y + s*b.x + c*b.y;
    out.yaw = a.yaw + b.yaw;
    // 归一化
    while (out.yaw >  M_PI) out.yaw -= 2*M_PI;
    while (out.yaw < -M_PI) out.yaw += 2*M_PI;
    return out;
  }

  static SE2 se2Between(const SE2& a, const SE2& b) {
    // a ⊖ b ：从 a 到 b 的相对位姿
    const double dx = b.x - a.x;
    const double dy = b.y - a.y;
    const double c = std::cos(-a.yaw), s = std::sin(-a.yaw);
    SE2 out;
    out.x   =  c*dx - s*dy;
    out.y   =  s*dx + c*dy;
    out.yaw =  b.yaw - a.yaw;
    while (out.yaw >  M_PI) out.yaw -= 2*M_PI;
    while (out.yaw < -M_PI) out.yaw += 2*M_PI;
    return out;
  }

  // 把基座系点用 SE2 变换到地图系
  static inline Reflector applySE2ToBasePoint(const SE2& T, const Reflector& b) {
    const double c = std::cos(T.yaw), s = std::sin(T.yaw);
    Reflector m;
    m.x = c*b.x - s*b.y + T.x;
    m.y = s*b.x + c*b.y + T.y;
    m.radius = b.radius;
    m.strength = b.strength;
    return m;
  }

  // 把雷达系点变到基座系（外参）旋转用yaw_lb，平移用t_lb
  static inline Reflector laserToBase(const Eigen::Vector2d& p_laser,
                                      const Eigen::Vector2d& t_lb,
                                      double yaw_lb,
                                      double radius, double strength) {
    const double c = std::cos(yaw_lb), s = std::sin(yaw_lb);
    Reflector b;
    b.x = c*p_laser.x() - s*p_laser.y() + t_lb.x();
    b.y = s*p_laser.x() + c*p_laser.y() + t_lb.y();
    b.radius = radius;
    b.strength = strength;
    return b;
  }

};

} // namespace SLAM

// ---------------- 主函数 ----------------
int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<SLAM::ReflectorDetectorNode>();
  rclcpp::executors::MultiThreadedExecutor exec;
  exec.add_node(node);
  exec.spin();
  rclcpp::shutdown();
  return 0;
}
